//ZHANG HAI JUN 710605420@qq.com

#include "Navigation.h"
#include <iostream>
#include <stdlib.h>
#include <cstring>
#include <memory>

#include "DetourTileCache.h"
#include "DetourCommon.h"

#include "Include/fastlz.h"

struct FastLZCompressor : public dtTileCacheCompressor
{
	virtual int maxCompressedSize(const int bufferSize)
	{
		return (int)(bufferSize * 1.05f);
	}

	virtual dtStatus compress(const unsigned char* buffer, const int bufferSize,
		unsigned char* compressed, const int /*maxCompressedSize*/, int* compressedSize)
	{
		*compressedSize = fastlz_compress((const void* const)buffer, bufferSize, compressed);
		return DT_SUCCESS;
	}

	virtual dtStatus decompress(const unsigned char* compressed, const int compressedSize,
		unsigned char* buffer, const int maxBufferSize, int* bufferSize)
	{
		*bufferSize = fastlz_decompress(compressed, compressedSize, buffer, maxBufferSize);
		return *bufferSize < 0 ? DT_FAILURE : DT_SUCCESS;
	}
};

struct MeshTempObstacleProcess : public dtTileCacheMeshProcess
{
	inline MeshTempObstacleProcess() {}

	virtual void process(struct dtNavMeshCreateParams* params,
		unsigned char* polyAreas, unsigned short* polyFlags)
	{
		// Update poly flags from areas.
		for (int i = 0; i < params->polyCount; ++i)
		{
			if (polyAreas[i] == DT_TILECACHE_WALKABLE_AREA)
				polyAreas[i] = SAMPLE_POLYAREA_GROUND;

			if (polyAreas[i] == SAMPLE_POLYAREA_GROUND ||
				polyAreas[i] == SAMPLE_POLYAREA_GRASS ||
				polyAreas[i] == SAMPLE_POLYAREA_ROAD)
			{
				polyFlags[i] = SAMPLE_POLYFLAGS_WALK;
			}
			else if (polyAreas[i] == SAMPLE_POLYAREA_WATER)
			{
				polyFlags[i] = SAMPLE_POLYFLAGS_SWIM;
			}
			else if (polyAreas[i] == SAMPLE_POLYAREA_DOOR)
			{
				polyFlags[i] = SAMPLE_POLYFLAGS_WALK | SAMPLE_POLYFLAGS_DOOR;
			}
		}
	}
};

struct NavMeshSetHeader
{
	int magic;
	int version;
	int numTiles;
	dtNavMeshParams params;
}; 

struct NavMeshTileHeader
{
	dtTileRef tileRef;
	int dataSize;
}; 


struct TileCacheSetHeader
{
	int magic;
	int version;
	int numTiles;
	dtNavMeshParams meshParams;
	dtTileCacheParams cacheParams;
};

struct TileCacheTileHeader
{
	dtCompressedTileRef tileRef;
	int dataSize;
};


static float frand()
{
	//	return ((float)(rand() & 0xffff)/(float)0xffff);
	return (float)rand() / (float)RAND_MAX;
}

Navigation::Navigation(){
	m_navMesh = nullptr;
	m_navQuery = nullptr;
	memset(m_polyPickExt, 0, sizeof(m_polyPickExt));
	memset(m_nearestPoint, 0, sizeof(m_nearestPoint));

	m_tileCache = nullptr;

	m_talloc = new LinearAllocator(32000);
	m_tcomp = new FastLZCompressor;
	m_tmproc = new MeshTempObstacleProcess;
}

dtNavMesh* Navigation::_LoadTempObstacleTile(const char* path)
{
	FILE* fp = fopen(path, "rb");
	if (!fp)
	{
		return nullptr;
	}

	// Read header.
	TileCacheSetHeader header;
	size_t headerReadReturnCode = fread(&header, sizeof(TileCacheSetHeader), 1, fp);
	if (headerReadReturnCode != 1)
	{
		// Error or early EOF
		fclose(fp);
		return nullptr;
	}
	if (header.magic != TILECACHESET_MAGIC)
	{
		fclose(fp);
		return nullptr;
	}
	if (header.version != TILECACHESET_VERSION)
	{
		fclose(fp);
		return nullptr;
	}

	m_navMesh = dtAllocNavMesh();
	if (!m_navMesh)
	{
		fclose(fp);
		return nullptr;
	}
	dtStatus status = m_navMesh->init(&header.meshParams);
	if (dtStatusFailed(status))
	{
		fclose(fp);
		return nullptr;
	}
	if (m_tileCache != nullptr)
	{
		dtFreeTileCache(m_tileCache);
		m_tileCache = nullptr;
	}
	m_tileCache = dtAllocTileCache();
	if (!m_tileCache)
	{
		fclose(fp);
		return nullptr;
	}
	status = m_tileCache->init(&header.cacheParams, m_talloc, m_tcomp, m_tmproc);
	if (dtStatusFailed(status))
	{
		fclose(fp);
		return nullptr;
	}

	// Read tiles.
	for (int i = 0; i < header.numTiles; ++i)
	{
		TileCacheTileHeader tileHeader;
		size_t tileHeaderReadReturnCode = fread(&tileHeader, sizeof(tileHeader), 1, fp);
		if (tileHeaderReadReturnCode != 1)
		{
			// Error or early EOF
			fclose(fp);
			return nullptr;
		}
		if (!tileHeader.tileRef || !tileHeader.dataSize)
			break;

		unsigned char* data = (unsigned char*)dtAlloc(tileHeader.dataSize, DT_ALLOC_PERM);
		if (!data) break;
		memset(data, 0, tileHeader.dataSize);
		size_t tileDataReadReturnCode = fread(data, tileHeader.dataSize, 1, fp);
		if (tileDataReadReturnCode != 1)
		{
			// Error or early EOF
			dtFree(data);
			fclose(fp);
			return nullptr;
		}

		dtCompressedTileRef tile = 0;
		dtStatus addTileStatus = m_tileCache->addTile(data, tileHeader.dataSize, DT_COMPRESSEDTILE_FREE_DATA, &tile);
		if (dtStatusFailed(addTileStatus))
		{
			dtFree(data);
		}

		if (tile)
			m_tileCache->buildNavMeshTile(tile, m_navMesh);
	}

	fclose(fp);
	return m_navMesh;
}

const int Navigation::GetTitleIndex(float* startPos)
{
	dtPolyRef polyRef;
	dtStatus status = m_navQuery->findNearestPoly(startPos, m_polyPickExt, &m_filter, &polyRef, 0);
	if (IsStatusSucceed(status))
	{
		if (nullptr != m_navMesh)
		{
			return m_navMesh->getTileIndex(polyRef);
		}
	}
	return -1;
}

const dtMeshTile* Navigation::GetTileByIndexAt(int i) const
{
	if (nullptr != m_navMesh)
	{
		return  const_cast<const dtNavMesh*>(m_navMesh)->getTile(i);
	}
	return nullptr;
}

int  Navigation::GetTilesAt(const int x, const int y,
	dtMeshTile const** tiles, const int maxTiles) const 
{
	if (nullptr != m_navMesh)
	{
		return  m_navMesh->getTilesAt(x, y, const_cast<const dtMeshTile**>(tiles), maxTiles);
	}
	return DT_FAILURE;
}

dtNavMesh* Navigation::_LoadTempObstacleTileFromBuffer(unsigned char* data, size_t dataSize)
{
	size_t pos = 0;
	size_t sz = sizeof(TileCacheSetHeader);
	if (!data || sz > dataSize)
		return nullptr;
	TileCacheSetHeader header;
	memcpy(&header, data, sz);
	pos += sz;
	dtNavMesh* mesh = dtAllocNavMesh();
	if (!mesh)
	{
		return nullptr;
	}
	dtStatus status = mesh->init(&header.meshParams);
	if (dtStatusFailed(status))
	{
		dtFreeNavMesh(mesh);
		mesh = nullptr;
		return nullptr;
	}
	if (m_tileCache != nullptr)
	{
		dtFreeTileCache(m_tileCache);
		m_tileCache = nullptr;
	}
	m_tileCache = dtAllocTileCache();
	if (!m_tileCache)
	{
		return nullptr;
	}
	status = m_tileCache->init(&header.cacheParams, m_talloc, m_tcomp, m_tmproc);
	if (dtStatusFailed(status))
	{
		return nullptr;
	}

	// Read tiles.
	for (int i = 0; i < header.numTiles; ++i)
	{
		TileCacheTileHeader tileHeader;
		sz = sizeof(TileCacheTileHeader);
		memcpy(&tileHeader, &data[pos], sz);
		pos += sz;
		sz = tileHeader.dataSize;
		if (!tileHeader.tileRef || !tileHeader.dataSize)
		{
			dtFreeNavMesh(mesh);
			mesh = nullptr;
			return nullptr;
		}

		unsigned char* tileData = (unsigned char*)dtAlloc(tileHeader.dataSize, DT_ALLOC_PERM);
		if (!tileData) break;
		memcpy(tileData, &data[pos], sz);
		pos += sz;

		dtCompressedTileRef tile = 0;
		dtStatus addTileStatus = m_tileCache->addTile(tileData, tileHeader.dataSize, DT_COMPRESSEDTILE_FREE_DATA, &tile);
		if (dtStatusFailed(addTileStatus))
		{
			if (tileData)
				dtFree(tileData);
			dtFreeNavMesh(mesh);
			mesh = nullptr;
		}

		if (tile)
			m_tileCache->buildNavMeshTile(tile, m_navMesh);

	}
	return mesh;
}

dtNavMesh* Navigation::_LoadTile( char* path)
{
	FILE* fp = fopen(path, "rb");
	if (!fp) return 0;

	// Read header.
	NavMeshSetHeader header;
	size_t readLen = fread(&header, sizeof(NavMeshSetHeader), 1, fp);
	if (readLen != 1)
	{
		fclose(fp);
		return 0;
	}
	if (header.magic != NAVMESHSET_MAGIC)
	{
		fclose(fp);
		return 0;
	}
	if (header.version != NAVMESHSET_VERSION)
	{
		fclose(fp);
		return 0;
	}

	dtNavMesh* mesh = dtAllocNavMesh();
	if (!mesh)
	{
		fclose(fp);
		return 0;
	}
	dtStatus status = mesh->init(&header.params);
	if (dtStatusFailed(status))
	{
		fclose(fp);
		return 0;
	}

	// Read tiles.
	for (int i = 0; i < header.numTiles; ++i)
	{
		NavMeshTileHeader tileHeader;
		readLen = fread(&tileHeader, sizeof(tileHeader), 1, fp);
		if (readLen != 1)
		{
			fclose(fp);
			return 0;
		}

		if (!tileHeader.tileRef || !tileHeader.dataSize)
			break;

		unsigned char* data = (unsigned char*)dtAlloc(tileHeader.dataSize, DT_ALLOC_PERM);
		if (!data) break;
		memset(data, 0, tileHeader.dataSize);
		readLen = fread(data, tileHeader.dataSize, 1, fp);
		if (readLen != 1)
		{
			dtFree(data);
			fclose(fp);
			return 0;
		}

		mesh->addTile(data, tileHeader.dataSize, DT_TILE_FREE_DATA, tileHeader.tileRef, 0);
	}

	fclose(fp);

	return mesh;
}

dtNavMesh* Navigation::_LoadTileFromBuffer(unsigned char* data, size_t dataSize)
{
	
	// Read header.
	
	size_t pos = 0;
	size_t sz = sizeof(NavMeshSetHeader);
	if (!data || sz > dataSize)
		return 0;
	NavMeshSetHeader header;
	memcpy(&header, data, sz);
	pos += sz;
	dtNavMesh* mesh = dtAllocNavMesh();
	if (!mesh)
	{
		return 0;
	}
	dtStatus status = mesh->init(&header.params);
	if (dtStatusFailed(status))
	{
		dtFreeNavMesh(mesh);
		mesh = nullptr;
		return 0;
	}
	// Read tiles.
	for (int i = 0; i < header.numTiles; ++i)
	{
		NavMeshTileHeader tileHeader;
		sz = sizeof(NavMeshTileHeader);
		memcpy(&tileHeader, &data[pos], sz);
		pos += sz;
		sz = tileHeader.dataSize;
		if (!tileHeader.tileRef || !tileHeader.dataSize)
		{
			dtFreeNavMesh(mesh);
			mesh = nullptr;
			return 0;
		}
		

		unsigned char* tileData = (unsigned char*)dtAlloc(tileHeader.dataSize, DT_ALLOC_PERM);
		if (!tileData) break;
		memcpy(tileData, &data[pos],sz);
		pos += sz;
		

		dtStatus status = mesh->addTile(tileData, tileHeader.dataSize, DT_TILE_FREE_DATA, tileHeader.tileRef, 0);
		if (!dtStatusSucceed(status))
		{
			if (tileData)
				dtFree(tileData);
			dtFreeNavMesh(mesh);
			mesh = nullptr;
			return 0;
		}
	}
// 	m_navQuery = dtAllocNavMeshQuery();
// 	if (!m_navQuery)
// 	{
// 		dtFreeNavMesh(mesh);
// 		mesh = nullptr;
// 		return 0;
// 	}
// 	status = m_navQuery->init(mesh, 2048);
// 	if (!dtStatusSucceed(status))
// 	{
// 		dtFreeNavMesh(mesh);
// 		mesh = nullptr;
// 		dtFreeNavMeshQuery(m_navQuery);
// 		m_navQuery = nullptr;
// 
// 		return 0;
// 	}
	return mesh;
}

const dtMeshTile* Navigation::GetTileAt(const int x, const int y, const int layer) const
{
	if (m_navMesh == nullptr)
	{
		return nullptr;
	}
	//m_tileCache->getTileAt(x, y, layer);
	return	m_navMesh->getTileAt(x, y, layer);
}

bool Navigation::LoadTileFromBuffer( unsigned char* data, size_t size)
{
	if (m_navQuery != nullptr)
	{
		dtFreeNavMeshQuery(m_navQuery);
		m_navQuery = nullptr;
	}
	if (m_navMesh != nullptr)
	{
		dtFreeNavMesh(m_navMesh);
		m_navMesh = nullptr;
	}
	m_neighbourhoodRadius = 2.5f;
	

	m_navMesh = _LoadTileFromBuffer(data,size);
	
	if (!m_navMesh)
		return false;

	m_navQuery = dtAllocNavMeshQuery();
	m_pathFindStatus = m_navQuery->init(m_navMesh, 2048);

	m_filter.setIncludeFlags(NAVIGATION_POLYFLAGS_ALL ^ NAVIGATION_POLYFLAGS_DISABLED);
	m_filter.setExcludeFlags(0);

	m_filter.setAreaCost(NAVIGATION_POLYAREA_GROUND, 1.0f);
	m_filter.setAreaCost(NAVIGATION_POLYAREA_WATER, 10.0f);
	m_filter.setAreaCost(NAVIGATION_POLYAREA_ROAD, 1.0f);
	m_filter.setAreaCost(NAVIGATION_POLYAREA_DOOR, 1.0f);
	m_filter.setAreaCost(NAVIGATION_POLYAREA_GRASS, 2.0f);
	m_filter.setAreaCost(NAVIGATION_POLYAREA_JUMP, 1.5f);

	m_polyPickExt[0] = 2;
	m_polyPickExt[1] = 4;
	m_polyPickExt[2] = 2;
	return true;
}

void Navigation::LoadTile( char* path)
{
	if (m_navQuery != nullptr)
	{
		dtFreeNavMeshQuery(m_navQuery);
		m_navQuery = nullptr;
	}
	if (m_navMesh != nullptr)
	{
		dtFreeNavMesh(m_navMesh);
		m_navMesh = nullptr;
	}
	
	m_neighbourhoodRadius = 2.5f;

	m_navMesh = _LoadTile(path);
	if (!m_navMesh)
		return;

	m_navQuery = dtAllocNavMeshQuery();
	m_pathFindStatus = m_navQuery->init(m_navMesh, 2048);

	m_filter.setIncludeFlags(NAVIGATION_POLYFLAGS_ALL ^ NAVIGATION_POLYFLAGS_DISABLED);
	m_filter.setExcludeFlags(0);

	m_filter.setAreaCost(NAVIGATION_POLYAREA_GROUND, 1.0f);
	m_filter.setAreaCost(NAVIGATION_POLYAREA_WATER, 10.0f);
	m_filter.setAreaCost(NAVIGATION_POLYAREA_ROAD, 1.0f);
	m_filter.setAreaCost(NAVIGATION_POLYAREA_DOOR, 1.0f);
	m_filter.setAreaCost(NAVIGATION_POLYAREA_GRASS, 2.0f);
	m_filter.setAreaCost(NAVIGATION_POLYAREA_JUMP, 1.5f);

	m_polyPickExt[0] = 2;
	m_polyPickExt[1] = 4;
	m_polyPickExt[2] = 2;

	
// 	m_neighbourhoodRadius = sample->getAgentRadius() * 20.0f;
// 	m_randomRadius = sample->getAgentRadius() * 30.0f;
}

bool Navigation::LoadTempObstacleTile(const char* path)
{
	if (m_navQuery != nullptr)
	{
		dtFreeNavMeshQuery(m_navQuery);
		m_navQuery = nullptr;
	}
	if (m_navMesh != nullptr)
	{
		dtFreeNavMesh(m_navMesh);
		m_navMesh = nullptr;
	}

	m_neighbourhoodRadius = 2.5f;

	m_navMesh = _LoadTempObstacleTile(path);
	if (!m_navMesh)
		return false;

	m_navQuery = dtAllocNavMeshQuery();
	m_pathFindStatus = m_navQuery->init(m_navMesh, 2048);

	m_filter.setIncludeFlags(NAVIGATION_POLYFLAGS_ALL ^ NAVIGATION_POLYFLAGS_DISABLED);
	m_filter.setExcludeFlags(0);

	m_filter.setAreaCost(NAVIGATION_POLYAREA_GROUND, 1.0f);
	m_filter.setAreaCost(NAVIGATION_POLYAREA_WATER, 10.0f);
	m_filter.setAreaCost(NAVIGATION_POLYAREA_ROAD, 1.0f);
	m_filter.setAreaCost(NAVIGATION_POLYAREA_DOOR, 1.0f);
	m_filter.setAreaCost(NAVIGATION_POLYAREA_GRASS, 2.0f);
	m_filter.setAreaCost(NAVIGATION_POLYAREA_JUMP, 1.5f);

	m_polyPickExt[0] = 2;
	m_polyPickExt[1] = 4;
	m_polyPickExt[2] = 2;
	
	return true;
}

bool Navigation::LoadTempObstacleTileFromBuffer(unsigned char* data, size_t size)
{
	return true;
}
dtStatus Navigation::AddObstacle(const float* pos, const float radius, const float height, dtObstacleRef* result)
{
	if (m_tileCache)
	{
		dtStatus status = m_tileCache->addObstacle(pos, radius, height, result);
		if (!IsStatusSucceed(status))
		{
			return status;
		}
		m_tileCache->update(0, m_navMesh);
		m_tileCache->update(0, m_navMesh);
		return status;
	}
	return DT_FAILURE;
}
// Aabb obstacle. notice height is not same int number
dtStatus Navigation::AddBoxObstacle(const float* bmin, const float* bmax, dtObstacleRef* result)
{
	if (m_tileCache)
	{
		dtStatus status = m_tileCache->addBoxObstacle(bmin, bmax, result);
		m_tileCache->update(0, m_navMesh);
		m_tileCache->update(0, m_navMesh);
		return status;
	}
	return DT_FAILURE;
}

// Box obstacle: can be rotated in Y.
dtStatus Navigation::AddBoxObstacle(const float* center, const float* halfExtents, const float yRadians, dtObstacleRef* result)
{
	if (m_tileCache)
	{
		dtStatus status = m_tileCache->addBoxObstacle(center, halfExtents, yRadians, result);
		m_tileCache->update(0, m_navMesh);
		m_tileCache->update(0, m_navMesh);
		return status;
	}
	return DT_FAILURE;
}

dtStatus Navigation::RemoveObstacle(const dtObstacleRef ref)
{
	if (m_tileCache)
	{
		return	m_tileCache->removeObstacle(ref);
	}
	return DT_FAILURE;
}
dtStatus Navigation::FindRaycast( float* startPos,  float* endPos, float* out_hitPos, float* out_tangent)
{
	if (m_navQuery == nullptr)
	{
		return DT_FAILURE;
	}
	dtPolyRef polyRef;
	dtStatus status = m_navQuery->findNearestPoly(startPos, m_polyPickExt, &m_filter, &polyRef, 0);
	if (!dtStatusSucceed(status)) { return status; }
	dtRaycastHit rayHit;
	rayHit.t = 1.0f;
	rayHit.maxPath = 0;
	status = m_navQuery->raycast(polyRef, startPos, endPos, &m_filter, 0, &rayHit);
	if (!dtStatusSucceed(status)) { return status; }
	if (rayHit.t < 1.0f)
	{
		dtVlerp(out_hitPos, startPos, endPos, rayHit.t);
		if (out_tangent)
		{
			float p[3] = { 0 };
			dtVsub(p, startPos, out_hitPos);
			float dir = (float)(p[2] * rayHit.hitNormal[0] - p[0] * rayHit.hitNormal[2] > 0 ? 1 : -1);
			out_tangent[0] = rayHit.hitNormal[2] * dir;
			out_tangent[1] = 0;
			out_tangent[2] = -rayHit.hitNormal[0] * dir;
		}
	}
	else
	{
		dtVcopy(out_hitPos, endPos);
	}
	return status;
}


bool Navigation::IsStatusSucceed(dtStatus status)
{
	return dtStatusSucceed(status);
}

dtStatus Navigation::FindNearestPoly( float* startPos, NavPoint* navPoint,  float *polyPickExt)
{
	if (m_navQuery == nullptr)
	{
		return DT_FAILURE;
	}
	dtStatus status;
	if (polyPickExt != nullptr)
	{
		status = m_navQuery->findNearestPoly(startPos, polyPickExt, &m_filter, &navPoint->polyRef, navPoint->point);
	}
	else
	{
		status = m_navQuery->findNearestPoly(startPos, m_polyPickExt, &m_filter, &navPoint->polyRef, navPoint->point);
	}
	return status;
}

dtStatus Navigation::FindPathStraight( float* startPos,  float* endPos, float *out_list, int* count)
{
	if (m_navQuery == nullptr)
	{
		return DT_FAILURE;
	}
	dtStatus startStatus = m_navQuery->findNearestPoly(startPos, m_polyPickExt, &m_filter, &m_startRef, 0);
	if (!dtStatusSucceed(startStatus)) { return startStatus; }
	dtStatus endStatus = m_navQuery->findNearestPoly(endPos, m_polyPickExt, &m_filter, &m_endRef, 0);
	if (!dtStatusSucceed(endStatus)) { return endStatus; }
	dtStatus queryStatus = m_navQuery->findPath(m_startRef, m_endRef, startPos, endPos, &m_filter, m_polys, &m_npolys, MAX_POLYS);
	if (!dtStatusSucceed(queryStatus)) { return queryStatus; }
	m_nstraightPath = 0;
	if (m_npolys > 0)
	{
		// In case of partial path, make sure the end point is clamped to the last polygon.
		float epos[3];
		dtVcopy(epos, endPos);
		if (m_polys[m_npolys - 1] != m_endRef)
			m_navQuery->closestPointOnPoly(m_polys[m_npolys - 1], endPos, epos, 0);
		if (!dtStatusSucceed(endStatus)) { return endStatus; }
		m_straightPathOptions = 1;
		dtStatus status = m_navQuery->findStraightPath(startPos, epos, m_polys, m_npolys,
			m_straightPath, m_straightPathFlags,
			m_straightPathPolys, &m_nstraightPath, MAX_POLYS, m_straightPathOptions);
		if (dtStatusSucceed(status))
		{
			for (int i = 0; i < m_nstraightPath - 1; ++i)
			{
				out_list[i * 3] = m_straightPath[i * 3];
				out_list[i * 3 + 1] = m_straightPath[i * 3 + 1];
				out_list[i * 3 + 2] = m_straightPath[i * 3 + 2];
			}
			*count = m_nstraightPath;

			return status;
		}
	}
	return DT_FAILURE;
}

dtStatus Navigation::FindPolyHeight(float* startPos, float* height)
{
	if (m_navQuery == nullptr)
	{
		return DT_FAILURE;
	}
	dtPolyRef polyRef;
	dtStatus status = m_navQuery->findNearestPoly(startPos, m_polyPickExt, &m_filter, &polyRef, 0);
	if (IsStatusSucceed(status))
	{
		return m_navQuery->getPolyHeight(polyRef, startPos, height);
	}
	return DT_FAILURE;
}

dtStatus Navigation::GetPolyFlags(float* startPos, unsigned short* resultFlags) 
{
	if (m_navMesh == nullptr)
	{
		return DT_FAILURE;
	}
	dtPolyRef polyRef;
	dtStatus status = m_navQuery->findNearestPoly(startPos, m_polyPickExt, &m_filter, &polyRef, 0);
	if (IsStatusSucceed(status))
	{
		return m_navMesh->getPolyFlags(polyRef, resultFlags);
	}
	return DT_FAILURE;
}

dtStatus Navigation::FindRandomPointAroundCircle(NavPoint centerPos,float radius,NavPoint* outPos)
{
	if (m_navQuery == nullptr)
	{
		return DT_FAILURE;
	}
	dtStatus status = m_navQuery->findRandomPointAroundCircle(centerPos.polyRef, centerPos.point, radius, &m_filter, frand, &outPos->polyRef, outPos->point);
	return status;
}

Navigation::~Navigation()
{
	if (m_talloc == nullptr)
	{
		delete m_talloc;
	}
	if (m_tcomp == nullptr)
	{
		delete m_tcomp;
	}
	if (m_tmproc == nullptr)
	{
		delete m_tmproc;
	}
	
	m_talloc = nullptr;
	m_tcomp = nullptr;
	m_tmproc = nullptr;

	if (m_tileCache != nullptr)
	{
		dtFreeTileCache(m_tileCache);
		m_tileCache = nullptr;
	}

	if (m_navQuery != nullptr)
		dtFreeNavMeshQuery(m_navQuery);
	m_navQuery = nullptr;

	if (m_navMesh != nullptr)
		dtFreeNavMesh(m_navMesh);
	m_navMesh = nullptr;

}